#ifndef SCARAMUZZACAMERA_H
#define SCARAMUZZACAMERA_H

#include <opencv2/core/core.hpp>
#include <string>

#include "ceres/rotation.h"
#include "Camera.h"

namespace camodocal
{

#define SCARAMUZZA_POLY_SIZE 5
#define SCARAMUZZA_INV_POLY_SIZE 20

#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/)

    /**
     * Scaramuzza Camera (Omnidirectional)
     * https://sites.google.com/site/scarabotix/ocamcalib-toolbox
     */

    class OCAMCamera : public Camera
    {
    public:
        class Parameters : public Camera::Parameters
        {
        public:
            Parameters();

            double &C(void) { return m_C; }
            double &D(void) { return m_D; }
            double &E(void) { return m_E; }

            double &center_x(void) { return m_center_x; }
            double &center_y(void) { return m_center_y; }

            double &poly(int idx) { return m_poly[idx]; }
            double &inv_poly(int idx) { return m_inv_poly[idx]; }

            double C(void) const { return m_C; }
            double D(void) const { return m_D; }
            double E(void) const { return m_E; }

            double center_x(void) const { return m_center_x; }
            double center_y(void) const { return m_center_y; }

            double poly(int idx) const { return m_poly[idx]; }
            double inv_poly(int idx) const { return m_inv_poly[idx]; }

            bool readFromYamlFile(const std::string &filename);
            void writeToYamlFile(const std::string &filename) const;

            Parameters &operator=(const Parameters &other);
            friend std::ostream &operator<<(std::ostream &out, const Parameters &params);

        private:
            double m_poly[SCARAMUZZA_POLY_SIZE];
            double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];
            double m_C;
            double m_D;
            double m_E;
            double m_center_x;
            double m_center_y;
        };

        OCAMCamera();

        /**
         * \brief Constructor from the projection model parameters
         */
        OCAMCamera(const Parameters &params);

        Camera::ModelType modelType(void) const;
        const std::string &cameraName(void) const;
        int imageWidth(void) const;
        int imageHeight(void) const;

        void estimateIntrinsics(const cv::Size &boardSize,
                                const std::vector<std::vector<cv::Point3f>> &objectPoints,
                                const std::vector<std::vector<cv::Point2f>> &imagePoints);

        // Lift points from the image plane to the sphere
        void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
        //%output P

        // Lift points from the image plane to the projective space
        void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
        //%output P

        // Projects 3D points to the image plane (Pi function)
        void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
        //%output p

        // Projects 3D points to the image plane (Pi function)
        // and calculates jacobian
        // void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
        //                  Eigen::Matrix<double,2,3>& J) const;
        //%output p
        //%output J

        void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
        //%output p

        template <typename T>
        static void spaceToPlane(const T *const params,
                                 const T *const q, const T *const t,
                                 const Eigen::Matrix<T, 3, 1> &P,
                                 Eigen::Matrix<T, 2, 1> &p);
        template <typename T>
        static void spaceToSphere(const T *const params,
                                  const T *const q, const T *const t,
                                  const Eigen::Matrix<T, 3, 1> &P,
                                  Eigen::Matrix<T, 3, 1> &P_s);
        template <typename T>
        static void LiftToSphere(const T *const params,
                                 const Eigen::Matrix<T, 2, 1> &p,
                                 Eigen::Matrix<T, 3, 1> &P);

        template <typename T>
        static void SphereToPlane(const T *const params, const Eigen::Matrix<T, 3, 1> &P,
                                  Eigen::Matrix<T, 2, 1> &p);

        void initUndistortMap(cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;
        cv::Mat initUndistortRectifyMap(cv::Mat &map1, cv::Mat &map2,
                                        float fx = -1.0f, float fy = -1.0f,
                                        cv::Size imageSize = cv::Size(0, 0),
                                        float cx = -1.0f, float cy = -1.0f,
                                        cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;

        int parameterCount(void) const;

        const Parameters &getParameters(void) const;
        void setParameters(const Parameters &parameters);

        void readParameters(const std::vector<double> &parameterVec);
        void writeParameters(std::vector<double> &parameterVec) const;

        void writeParametersToYamlFile(const std::string &filename) const;

        std::string parametersToString(void) const;

    private:
        Parameters mParameters;

        double m_inv_scale;
    };

    typedef boost::shared_ptr<OCAMCamera> OCAMCameraPtr;
    typedef boost::shared_ptr<const OCAMCamera> OCAMCameraConstPtr;

    template <typename T>
    void
    OCAMCamera::spaceToPlane(const T *const params,
                             const T *const q, const T *const t,
                             const Eigen::Matrix<T, 3, 1> &P,
                             Eigen::Matrix<T, 2, 1> &p)
    {
        T P_c[3];
        {
            T P_w[3];
            P_w[0] = T(P(0));
            P_w[1] = T(P(1));
            P_w[2] = T(P(2));

            // Convert quaternion from Eigen convention (x, y, z, w)
            // to Ceres convention (w, x, y, z)
            T q_ceres[4] = {q[3], q[0], q[1], q[2]};

            ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);

            P_c[0] += t[0];
            P_c[1] += t[1];
            P_c[2] += t[2];
        }

        T c = params[0];
        T d = params[1];
        T e = params[2];
        T xc[2] = {params[3], params[4]};

        // T poly[SCARAMUZZA_POLY_SIZE];
        // for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
        //     poly[i] = params[5+i];

        T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
        for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
            inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];

        T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
        T norm = T(0.0);
        if (norm_sqr > T(0.0))
            norm = sqrt(norm_sqr);

        T theta = atan2(-P_c[2], norm);
        T rho = T(0.0);
        T theta_i = T(1.0);

        for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
        {
            rho += theta_i * inv_poly[i];
            theta_i *= theta;
        }

        T invNorm = T(1.0) / norm;
        T xn[2] = {
            P_c[0] * invNorm * rho,
            P_c[1] * invNorm * rho};

        p(0) = xn[0] * c + xn[1] * d + xc[0];
        p(1) = xn[0] * e + xn[1] + xc[1];
    }

    template <typename T>
    void
    OCAMCamera::spaceToSphere(const T *const params,
                              const T *const q, const T *const t,
                              const Eigen::Matrix<T, 3, 1> &P,
                              Eigen::Matrix<T, 3, 1> &P_s)
    {
        T P_c[3];
        {
            T P_w[3];
            P_w[0] = T(P(0));
            P_w[1] = T(P(1));
            P_w[2] = T(P(2));

            // Convert quaternion from Eigen convention (x, y, z, w)
            // to Ceres convention (w, x, y, z)
            T q_ceres[4] = {q[3], q[0], q[1], q[2]};

            ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);

            P_c[0] += t[0];
            P_c[1] += t[1];
            P_c[2] += t[2];
        }

        // T poly[SCARAMUZZA_POLY_SIZE];
        // for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
        //     poly[i] = params[5+i];

        T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];
        T norm = T(0.0);
        if (norm_sqr > T(0.0))
            norm = sqrt(norm_sqr);

        P_s(0) = P_c[0] / norm;
        P_s(1) = P_c[1] / norm;
        P_s(2) = P_c[2] / norm;
    }

    template <typename T>
    void
    OCAMCamera::LiftToSphere(const T *const params,
                             const Eigen::Matrix<T, 2, 1> &p,
                             Eigen::Matrix<T, 3, 1> &P)
    {
        T c = params[0];
        T d = params[1];
        T e = params[2];
        T cc[2] = {params[3], params[4]};
        T poly[SCARAMUZZA_POLY_SIZE];
        for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
            poly[i] = params[5 + i];

        // Relative to Center
        T p_2d[2];
        p_2d[0] = T(p(0));
        p_2d[1] = T(p(1));

        T xc[2] = {p_2d[0] - cc[0], p_2d[1] - cc[1]};

        T inv_scale = T(1.0) / (c - d * e);

        // Affine Transformation
        T xc_a[2];

        xc_a[0] = inv_scale * (xc[0] - d * xc[1]);
        xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);

        T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];
        T phi = sqrt(norm_sqr);
        T phi_i = T(1.0);
        T z = T(0.0);

        for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
        {
            if (i != 1)
            {
                z += phi_i * poly[i];
            }
            phi_i *= phi;
        }

        T p_3d[3];
        p_3d[0] = xc[0];
        p_3d[1] = xc[1];
        p_3d[2] = -z;

        T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];
        T p_3d_norm = sqrt(p_3d_norm_sqr);

        P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;
    }

    template <typename T>
    void OCAMCamera::SphereToPlane(const T *const params, const Eigen::Matrix<T, 3, 1> &P,
                                   Eigen::Matrix<T, 2, 1> &p)
    {
        T P_c[3];
        {
            P_c[0] = T(P(0));
            P_c[1] = T(P(1));
            P_c[2] = T(P(2));
        }

        T c = params[0];
        T d = params[1];
        T e = params[2];
        T xc[2] = {params[3], params[4]};

        T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
        for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
            inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];

        T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
        T norm = T(0.0);
        if (norm_sqr > T(0.0))
            norm = sqrt(norm_sqr);

        T theta = atan2(-P_c[2], norm);
        T rho = T(0.0);
        T theta_i = T(1.0);

        for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
        {
            rho += theta_i * inv_poly[i];
            theta_i *= theta;
        }

        T invNorm = T(1.0) / norm;
        T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};

        p(0) = xn[0] * c + xn[1] * d + xc[0];
        p(1) = xn[0] * e + xn[1] + xc[1];
    }
}

#endif
